#include <kernel.h>

#define LD_NET     0
#define USER       1
#define USER_DEBUG 0

#if USER

typedef
int(*mmoveproc_t)(char *dest, char *base, size_t offset, size_t fillsize, size_t copysize);

/* simple routine for loading ELF */
static int
load_elf_image(struct elf_s *image, mmoveproc_t mmove)
{
     struct proghdr_s *ph, *eph;
	 int err = 0;
	 
     if (image->e_magic != ELF_MAGIC)
		  return -E_INVAL;

     ph  = (struct proghdr_s *)((char *)image + image->e_phoff);
     eph = ph + image->e_phnum;

     for ( ; ph != eph; ++ph)
     {
		  if (ph->p_type == ELF_PROG_LOAD)
		  {
			   if ((err = mmove((char *)ph->p_va, (char *)image,
								ph->p_offset, ph->p_memsz, ph->p_filesz))
				   != 0)
			   {
					break;
			   }
		  }
     }

	 return err;
}

int
my_move(char *dest, char *base, size_t offset, size_t fillsize, size_t copysize)
{
#if USER_DEBUG
     ekf_kprintf(" -- trying to move [%x, %x] to [%x, %x]\n",
				 base + offset, base + offset + copysize,
				 dest, dest + fillsize);
#endif

     base = base + offset; offset = 0;

     uintptr_t r = (uintptr_t)dest & (PAGE_SIZE - 1);     
     int err;

     uint32_t perm;
     
     if (r)
     {
		  r = PAGE_SIZE - r;
		  perm = PTE_W | PTE_U;
		  if ((err = ekf_fix_addr(dest, &perm, false)) != 0)
		  {
			   return err;
		  }
		       
		  if (r > copysize)
		  {
			   memmove(dest, base, copysize);
			   memset(dest, 0, r - copysize);
	       
		  }
		  else
		  {
			   memmove(dest, base, r);
		  }
	  
		  dest += r;
		  base += r;
		  offset += r;
     }
     
     while (offset < fillsize)
     {
		  perm = PTE_W | PTE_U;
		  if ((err = ekf_fix_addr(dest, &perm, false)) != 0)
		  {
			   return err;
		  }

		  if (offset + PAGE_SIZE <= copysize)
			   memmove(dest, base, PAGE_SIZE);
		  else if (offset < copysize)
		  {
			   memmove(dest, base, copysize - offset);
			   memset(dest + copysize - offset, 0, PAGE_SIZE - copysize + offset);
		  }
		  else memset(dest, 0, PAGE_SIZE);

		  dest += PAGE_SIZE;
		  base += PAGE_SIZE;
		  offset += PAGE_SIZE;
     }

     return 0;
}

#endif

static void
__init_proc(void *arg)
{
	 kvstore_init();	 
	 hpet_timer_init();
	 timer_freq_measure();
	 ekf_kprintf("TIMER FREQ: %d\n", timer_freq_get());
	 // In graphics mode we do not use this :(
	 // monitor_init();
	 vesa_init();

	 while (1) ;

#if LD_NET
	 void lwip_ekos_init(void);
	 void __LD_init(void);
	 void echo_server_proc(void *arg);
	 
	 __LD_init();
	 lwip_ekos_init();

	 proc_attach(kthread_create("echo_server", 0, 0, echo_server_proc, NULL, 0));
#endif

#if USER
	 int errno;
	 extern char _binary_mod_main_user_start[];
	 
	 ekf_kprintf("Jump into userland\n");	 
	 /* load elf */
	 if ((errno = load_elf_image((struct elf_s *)_binary_mod_main_user_start, my_move)) != 0)
	 {
		  ekf_kprintf("Failed\n");
	 }
	 else
	 {
		  proc_attach(uthread_create("__uinit", 0, 0, proc_kvpt_get(),
									 ((struct elf_s *)_binary_mod_main_user_start)->e_entry, 0, 0, 0));
	 }
#endif

#if USE_IDLE
	 proc_exit();
#else
	 proc_do_idle();
#endif
}

static void
__idle_proc(void *arg)
{
	 proc_attach(proc_create("__init", 0, 0, proc_kvpt_get(), __init_proc, NULL));
	 proc_do_idle();
}

static void
_entry(void)
{
	 intr_init();
#if USE_IDLE
	 proc_init("__idle", 0, __idle_proc, NULL);
#else
	 proc_init("__init", 0, __init_proc, NULL);
#endif
}

/* ============================================================ */

int
module_load(void **arg)
{
	 *arg = _entry;
	 return 0;
}

int
module_unload(void)
{
	 return 0;
}
